#include "Malvin.h"
#include "pal/palFactory.h"
#include "pal/ConfigStatic.h"
#include "elementconfig.h"
namespace robots
{

Malvin::Malvin()
{
	mainBody = 0;
	head = 0;
	bodyElement = 0;
	headElement = 0;
}

Malvin::~Malvin()
{
	delete mainBody;
	delete head;
	delete bodyElement;
	delete headElement;
}

int Malvin::getElementCount()
{
	return 2;
}

ElementData Malvin::getElementData(int index)
{
	if (index == 0) {
		palMatrix4x4 loc = mainBody->GetLocationMatrix();
		bodyElement->setTransformationMatrix(loc._mat);
		return *bodyElement;
	}
	if (index == 1) {
		palMatrix4x4 loc = head->GetLocationMatrix();
		headElement->setTransformationMatrix(loc._mat);
		return *headElement;
	}
}

void Malvin::move(float x, float y, float z)
{
//	mainBody->SetActive(true);
//	mainBody->ApplyImpulse(x, y, z);
//	head->SetActive(true);
//	head->ApplyImpulse(x*2, y*2, z*2);
}

void Malvin::command(int commandID, int n, void* args){
			head->SetActive(true);
	switch(commandID){
		case 1:
			head->ApplyImpulse(0, 0, -2);
  		break;		  
		case 2:
			head->ApplyImpulse(0, 0, 2);
  		break;
		case 3:
			head->ApplyImpulse(-2, 0, 0);
  		break;
		case 4:
			head->ApplyImpulse(2, 0, 0);
  		break;
	}
}

void Malvin::setPosition(float x, float y, float z)
{
	position.x = x;
	position.y = y;
	position.z = z;
}

palVector3 Malvin::getPosition()
{
	palVector3 pos;
	if (head != 0)
	{
		mainBody->GetPosition(pos);
//		head->GetPosition(pos);
	}
	else
	{
		pos.x = position.x;
		pos.y = position.y;
		pos.z = position.z;
	}
	return pos;
}

void Malvin::createPhysics()
{
	float* edata;
	mainBody = PF->CreateBox();
	mainBody->Init(position.x, position.y, position.z, 1, 1, 1, 1);
	bodyElement = new ElementData(CubeType);
	edata = new float[3];
	edata[0] = 1;
	edata[1] = 1;
	edata[2] = 1;
	bodyElement->setData(3, edata);
	bodyElement->setColor(155,155,255);
	palMatrix4x4 loc = mainBody->GetLocationMatrix();
	bodyElement->setTransformationMatrix(loc._mat);
	delete[] edata;
	head = PF->CreateSphere();
	head->Init(position.x, position.y + 1, position.z, 0.8, 1);
	headElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.8;
	headElement->setData(1, edata);
	headElement->setColor(0,0,255);
	delete[] edata;
	palMatrix4x4 loc2 = head->GetLocationMatrix();
	headElement->setTransformationMatrix(loc2._mat);
}

}
